﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Xml;
using HalconDotNet;

namespace fhs
{
    class FlatCalib
    {
        readonly bool _isProjective;
        HTuple _homMat2D;
        HTuple _revHomMat2D;
        LinkedList<Tuple<MyPoint, MyPoint>> _pq = new LinkedList<Tuple<MyPoint, MyPoint>>();

        public FlatCalib(bool isProjective = false)
        {
            _isProjective = isProjective;
        }

        public void BeginCalib()
        {
            _homMat2D = null;
            _revHomMat2D = null;
            _pq.Clear();
        }

        public void EndCalib()
        {
            HTuple px = new HTuple(), py = new HTuple();
            HTuple qx = new HTuple(), qy = new HTuple();
            foreach (Tuple<MyPoint, MyPoint> ptItem in _pq)
            {
                px.Append(ptItem.Item1.X);
                py.Append(ptItem.Item1.Y);
                qx.Append(ptItem.Item2.X);
                qy.Append(ptItem.Item2.Y);
            }
            if (_isProjective)
            {
                HTuple covariance;
                HTuple emptyCov = new HTuple();
                HOperatorSet.VectorToProjHomMat2d(px, py, qx, qy, "gold_standard",
                    emptyCov, emptyCov, emptyCov, emptyCov, emptyCov, emptyCov,
                    out _homMat2D, out covariance);
                HOperatorSet.VectorToProjHomMat2d(qx, qy, px, py, "gold_standard",
                    emptyCov, emptyCov, emptyCov, emptyCov, emptyCov, emptyCov,
                    out _revHomMat2D, out covariance);
            }
            else
            {
                HOperatorSet.VectorToHomMat2d(px, py, qx, qy, out _homMat2D);
                HOperatorSet.VectorToHomMat2d(qx, qy, px, py, out _revHomMat2D);
            }
        }

        public double Residual(bool reverse = false)
        {
            double maxErr = 0;
            foreach (Tuple<MyPoint, MyPoint> ptItem in _pq)
            {
                double err = (ptItem.Item2 - Trans(ptItem.Item1, reverse)).Mod;
                if (err > maxErr)
                {
                    maxErr = err;
                }
            }
            return maxErr;
        }

        public void Append(MyPoint p, MyPoint q)
        {
            _pq.AddLast(new Tuple<MyPoint, MyPoint>(p, q));
        }

        public void Append(double px, double py, double qx, double qy)
        {
            Append(new MyPoint { X = px, Y = py }, new MyPoint { X = qx, Y = qy });
        }

        public void Save(string path)
        {
            XmlDocument doc = new XmlDocument();
            XmlElement root = doc.CreateElement("FlatCalib");
            doc.AppendChild(root);
            foreach (Tuple<MyPoint, MyPoint> ptItem in _pq)
            {
                XmlElement point = doc.CreateElement("Point");
                point.SetAttribute("px", ptItem.Item1.X.ToString());
                point.SetAttribute("py", ptItem.Item1.Y.ToString());
                point.SetAttribute("qx", ptItem.Item2.X.ToString());
                point.SetAttribute("qy", ptItem.Item2.Y.ToString());
                root.AppendChild(point);
            }
            doc.Save(path);
        }

        public void Load(string path)
        {
            BeginCalib();
            XmlDocument doc = new XmlDocument();
            doc.Load(path);
            XmlElement root = doc.DocumentElement;
            foreach (XmlNode ptItem in root.ChildNodes)
            {
                Append(double.Parse(((XmlElement)ptItem).GetAttribute("px")), double.Parse(((XmlElement)ptItem).GetAttribute("py")),
                    double.Parse(((XmlElement)ptItem).GetAttribute("qx")), double.Parse(((XmlElement)ptItem).GetAttribute("qy")));
            }
            EndCalib();
        }

        public MyPoint Trans(MyPoint p, bool reverse = false)
        {
            if (_isProjective)
            {
                HTuple qx, qy, qw;
                HOperatorSet.ProjectiveTransPoint2d(reverse ? _revHomMat2D : _homMat2D, p.X, p.Y, 1, out qx, out qy, out qw);
                return new MyPoint { X = (double)qx / (double)qw, Y = (double)qy / (double)qw };
            }
            else
            {
                HTuple qx, qy;
                HOperatorSet.AffineTransPoint2d(reverse ? _revHomMat2D : _homMat2D, p.X, p.Y, out qx, out qy);
                return new MyPoint { X = qx, Y = qy };
            }
        }

        public void Trans(double px, double py, out double qx, out double qy, bool reverse = false)
        {
            MyPoint q = Trans(new MyPoint { X = px, Y = py }, reverse);
            qx = q.X; qy = q.Y;
        }

        static public MyPoint RotateCenter(MyPoint p0, MyPoint p1, double phi)
        {
            double halfDx = 0.5 * (p1.X - p0.X);
            double halfDy = 0.5 * (p1.Y - p0.Y);
            double cotHalfPhi = Math.Cos(0.5 * phi) / Math.Sin(0.5 * phi);
            return new MyPoint {
                X = p0.X + (halfDx - halfDy * cotHalfPhi),
                Y = p0.Y + (halfDy + halfDx * cotHalfPhi)
            };
        }

        static public MyPoint RotateCenterAng(MyPoint p0, MyPoint p1, double phiAng)
        {
            return RotateCenter(p0, p1, phiAng * Math.PI / 180);
        }

        static public MyPoint RotatePoint(MyPoint o, MyPoint p, double phi)
        {
            double sinPhi = Math.Sin(phi);
            double cosPhi = Math.Cos(phi);
            double dx = p.X - o.X;
            double dy = p.Y - o.Y;
            return new MyPoint {
                X = o.X + (cosPhi * dx - sinPhi * dy),
                Y = o.Y + (sinPhi * dx + cosPhi * dy)
            };
        }

        static public MyPoint RotatePointAng(MyPoint o, MyPoint p, double phiAng)
        {
            return RotatePoint(o, p, phiAng * Math.PI / 180);
        }

        static public double NorAngle(double phiAng)
        {
            phiAng = phiAng % 360;
            if (phiAng > 180)
            {
                phiAng -= 360;
            }
            else if (phiAng < -180)
            {
                phiAng += 360;
            }
            return phiAng;
        }

        static public double NorPhi(double phi)
        {
            phi = phi % (2 * Math.PI);
            if (phi > Math.PI)
            {
                phi -= 2 * Math.PI;
            }
            else if (phi < -Math.PI)
            {
                phi += 2 * Math.PI;
            }
            return phi;
        }

        static public double VectorAngle(MyPoint vec)
        {
            return Math.Atan2(vec.Y, vec.X);
        }

        static public double VectorAngle(MyPoint vec1, MyPoint vec2)
        {
            return Math.Atan2(vec1.Y, vec1.X) - Math.Atan2(vec2.Y, vec2.X);
        }

        static public double LineAngle(MyPoint ls1, MyPoint ls2, MyPoint le1, MyPoint le2)
        {
            double phi = Math.Abs(NorPhi(VectorAngle(ls2 - ls1, le2 - le1)));
            if (phi > Math.PI / 2)
            {
                phi = Math.PI - phi;
            }
            return phi;
        }

        static public double ToRad(double deg)
        {
            return deg * (Math.PI / 180);
        }

        static public double ToDeg(double rad)
        {
            return rad * (180 / Math.PI);
        }
    }
}
